void update_command(float current, struct TCommande *param){
  param->old = param->current;
  param->current = current;
  param->diff = (param->current - param->old)*RADIO_LOOP;
}


void update_command_diff(float diff, struct TCommande *param){
  param->old = param->current;
  param->diff = diff;
  param->current += diff/RADIO_LOOP;

  param->current = normalize180(param->current);
}

//read the radio inputs
void readRadio(void){
  float aux_float;

  if (ServoDecode.getState() == READY_state){ // if decoder ready

    for(int i=1; i<=NB_CHANNELS; i++){
      ch[i] = ServoDecode.GetChannelPulseWidth(i);   // Read radio channel values
    }

    // Read commands from radio Rx
    update_command(REVERSE_THROTTLE(signed(ch[CHANN_THROTTLE])-MIN_CHANN)/(MAX_CHANN-MIN_CHANN), &command_throttle);

    // Stick position defines the desired angle in roll, pitch and a angular speed in yaw
    update_command(REVERSE_ROLL(signed(ch[CHANN_ROLL])-NEUTRAL_CHANN) *MAX_ROLL/(MAX_CHANN-NEUTRAL_CHANN), &command_roll);  //+- MAX_ROLL

    update_command(REVERSE_PITCH(signed(ch[CHANN_ELEVATOR])-NEUTRAL_CHANN) *MAX_PITCH/(MAX_CHANN-NEUTRAL_CHANN), &command_pitch);  //+- MAX_PITCH

    aux_float = REVERSE_YAW(signed(ch[CHANN_RUDDER])-NEUTRAL_CHANN) *MAX_YAW_SPEED/(MAX_CHANN-NEUTRAL_CHANN);  //+- MAX_YAW_SPEED
    if (abs(aux_float) < (MAX_YAW_SPEED/20))  //create a +-20 us dead zone around neutral position
      aux_float = 0.;
    update_command_diff(aux_float, &command_yaw);

    // Estimate speed (for centrifugal force correction)
    if (ch[CHANN_THROTTLE] > 1400) 
      speed_3d = CRUISE_SPEED; 
    else
      speed_3d = 0;

    //set mode
    if(ch[CHANN_MODE]<1300){ // Manual mode
      AP_mode = MANUAL;   
      digitalWrite(6, LOW); // Light off Blue led
    } 
    else if(ch[CHANN_MODE]>1700){ // Test mode
      AP_mode = TEST_MODE;   
     /* if (ch[CHANN_SWITCH] < 1500) {  //tune roll gains
        //tune proportionnal
        PIDroll.Pgain = KP_ROLL*0.5*exp((signed(ch[CHANN_KNOB1])-MIN_CHANN)/577.);  //between 0.5 and 2 times the default gains
        //PIDroll.Pgain = KP_ROLL*0.25*exp((signed(ch[CHANN_KNOB1])-MIN_CHANN)/289.);  //between 0.25 and 4 times the default gains
        
        //tune derivative
        PIDroll.Dgain = KD_ROLL*0.5*exp((signed(ch[CHANN_KNOB2])-MIN_CHANN)/577.);  //between 0.5 and 2 times the default gains
        //PIDroll.Dgain = KD_ROLL*0.25*exp((signed(ch[CHANN_KNOB2])-MIN_CHANN)/289.);  //between 0.25 and 4 times the default gains
        //tune pitch of roll
        //kpor = K_PITCHofROLL*0.25*exp((ch[6]-1100)/289.);
      } 
      else{  //tune pitch gains
        //tune proportionnal
        PIDpitch.Pgain = KP_PITCH*0.5*exp((signed(ch[CHANN_KNOB1])-MIN_CHANN)/577.);  //between 0.5 and 2 times the default gains
        //PIDpitch.Pgain = KP_PITCH*0.25*exp((signed(ch[CHANN_KNOB1])-MIN_CHANN)/289.);  //between 0.25 and 4 times the default gains
        
        //tune derivative
        PIDpitch.Dgain = KD_PITCH*0.5*exp((signed(ch[CHANN_KNOB2])-MIN_CHANN)/577.);  //between 0.5 and 2 times the default gains
        //PIDpitch.Dgain = KD_PITCH*0.25*exp((signed(ch[CHANN_KNOB2])-MIN_CHANN)/289.);  //between 0.25 and 4 times the default gains
      }*/
      /* else{  //tune yaw gains
       //tune proportionnal
       PIDyaw.Pgain = KP_YAW*0.25*exp((signed(ch[CHANN_KNOB1])-MIN_CHANN)/289.);
       //tune derivative
       PIDyaw.Dgain = KD_YAW*0.25*exp((signed(ch[CHANN_KNOB2])-MIN_CHANN)/289.);
       }*/
    } 
    else {  // Stabilization assist mode  
      AP_mode = FLY_BY_WIRE; 
      digitalWrite(6, HIGH); // Light on Blue led
    }
  } 
  else { //no radio
    digitalWrite(5, HIGH); // Light on Red led 
    digitalWrite(6, HIGH); // Light on Blue led
    update_command(FAILSAFE_ROLL, &command_roll);  //voir pour initialiser la diff a zero ?
    update_command(FAILSAFE_PITCH, &command_pitch);  //voir pour initialiser la diff a zero ?
    AP_mode = NO_RADIO;
    ch[CHANN_THROTTLE] = MIN_CHANN + 45; //cut throttle but not min throttle to active stabilization
  }
}


